#include <armadillo>

arma::Mat<double> TetrahedronElementStiffness(float E,float NU,arma::Mat<double> x1,arma::Mat<double> x2, arma::Mat<double> x3, arma::Mat<double> x4) ;
arma::Mat<double> calculateB(arma::Mat<double> x1,arma::Mat<double> x2, arma::Mat<double> x3, arma::Mat<double> x4);
arma::Mat<double> calculateD(float E,float NU);

void main(int argc, char *argv[])
{
	float E = 2300000.3;
	float vn = 0.33;

	arma::Mat<double> Ke(12,12);
	Ke = arma::zeros<arma::mat>(12,12);

	arma::Col<double> x0(3);
	arma::Col<double> x1(3);
	arma::Col<double> x2(3);
	arma::Col<double> x3(3);

	x0(0) = 3;
	x0(1) = 6;
	x0(2) = -2;

	x1(0) = 9;
	x1(1) = -13;
	x1(2) = 4;

	x2(0) = 5;
	x2(1) = 11;
	x2(2) = -7;

	x3(0) = -1;
	x3(1) = -8;
	x3(2) = 41;

	Ke = TetrahedronElementStiffness(E,vn, x0, x1, x2, x3);

	/*for (int i=0;i<12;i++)
	{
		for (int j=0;j<12;j++)
		{
			printf("%.02f ",Ke.at(i,j));
		}

		printf("\n");
	}*/
}

arma::Mat<double> TetrahedronElementStiffness(float E,float NU,arma::Mat<double> x1,arma::Mat<double> x2, arma::Mat<double> x3, arma::Mat<double> x4) 
{
	arma::Mat<double> xyz = arma::zeros(4,4);
	xyz << 1 << x1(0) << x1(1) << x1(2) << arma::endr
		<< 1 << x2(0) << x2(1) << x2(2) << arma::endr
		<< 1 << x3(0) << x3(1) << x3(2) << arma::endr
		<< 1 << x4(0) << x4(1) << x4(2) << arma::endr;

	double V = arma::det(xyz)/6;

	arma::Mat<double> B = arma::zeros(6,6);

	B = calculateB(x1, x2, x3,  x4);

	arma::Mat<double> D;

	D = calculateD(E,NU);

	for (int i=0;i<6;i++)
	{
		for (int j=0;j<6;j++)
		{
			printf("%f ",D.at(i,j));
		}
		printf("\n");
	}

	arma::Mat<double> K =  V*trans(B)*D*B;

	return K;
}

arma::Mat<double> calculateB(arma::Mat<double> x1,arma::Mat<double> x2, arma::Mat<double> x3, arma::Mat<double> x4) {

	arma::Mat<double> xyz = arma::zeros(4,4);
	xyz << 1 << x1(0) << x1(1) << x1(2) << arma::endr
		<< 1 << x2(0) << x2(1) << x2(2) << arma::endr
		<< 1 << x3(0) << x3(1) << x3(2) << arma::endr
		<< 1 << x4(0) << x4(1) << x4(2) << arma::endr;

	double V = arma::det(xyz)/6;

	arma::Mat<double> mbeta1 = arma::zeros(3,3);
	mbeta1 << 1 << x2(1) << x2(2) <<  arma::endr
		<< 1 << x3(1) << x3(2) <<  arma::endr
		<< 1 << x4(1) << x4(2) <<  arma::endr;

	arma::Mat<double> mbeta2 = arma::zeros(3,3);
	mbeta2 << 1 << x1(1) << x1(2) <<  arma::endr
		<< 1 << x3(1) << x3(2) <<  arma::endr
		<< 1 << x4(1) << x4(2) <<  arma::endr;

	arma::Mat<double> mbeta3 = arma::zeros(3,3);
	mbeta3 << 1 << x1(1) << x1(2) <<  arma::endr
		<< 1 << x2(1) << x2(2) <<  arma::endr
		<< 1 << x4(1) << x4(2) <<  arma::endr;

	arma::Mat<double> mbeta4 = arma::zeros(3,3);
	mbeta4 << 1 << x1(1) << x1(2) <<  arma::endr
		<< 1 << x2(1) << x2(2) <<  arma::endr
		<< 1 << x3(1) << x3(2) <<  arma::endr;

	arma::Mat<double> mgamma1 = arma::zeros(3,3);
	mgamma1 << 1 << x2(0) << x2(2) <<  arma::endr
		<< 1 << x3(0) << x3(2) <<  arma::endr
		<< 1 << x4(0) << x4(2) <<  arma::endr;

	arma::Mat<double> mgamma2 = arma::zeros(3,3);
	mgamma2 << 1 << x1(0) << x1(2) <<  arma::endr
		<< 1 << x3(0) << x3(2) <<  arma::endr
		<< 1 << x4(0) << x4(2) <<  arma::endr;

	arma::Mat<double> mgamma3 = arma::zeros(3,3);
	mgamma3 << 1 << x1(0) << x1(2) <<  arma::endr
		<< 1 << x2(0) << x2(2) <<  arma::endr
		<< 1 << x4(0) << x4(2) <<  arma::endr;

	arma::Mat<double> mgamma4 = arma::zeros(3,3);
	mgamma4 << 1 << x1(0) << x1(2) <<  arma::endr
		<< 1 << x2(0) << x2(2) <<  arma::endr
		<< 1 << x3(0) << x3(2) <<  arma::endr;

	arma::Mat<double> mdelta1 = arma::zeros(3,3);
	mdelta1 << 1 << x2(0) << x2(1) <<  arma::endr
		<< 1 << x3(0) << x3(1) <<  arma::endr
		<< 1 << x4(0) << x4(1) <<  arma::endr;

	arma::Mat<double> mdelta2 = arma::zeros(3,3);
	mdelta2 << 1 << x1(0) << x1(1) <<  arma::endr
		<< 1 << x3(0) << x3(1) <<  arma::endr
		<< 1 << x4(0) << x4(1) <<  arma::endr;

	arma::Mat<double> mdelta3 = arma::zeros(3,3);
	mdelta3 << 1 << x1(0) << x1(1) <<  arma::endr
		<< 1 << x2(0) << x2(1) <<  arma::endr
		<< 1 << x4(0) << x4(1) <<  arma::endr;

	arma::Mat<double> mdelta4 = arma::zeros(3,3);
	mdelta4 << 1 << x1(0) << x1(1) <<  arma::endr
		<< 1 << x2(0) << x2(1) <<  arma::endr
		<< 1 << x3(0) << x3(1) <<  arma::endr;

	double beta1 = -1*det(mbeta1);


	double beta2 = det(mbeta2);
	double beta3 = -1*det(mbeta3);
	double beta4 = det(mbeta4);
	double gamma1 = det(mgamma1);
	double gamma2 = -1*det(mgamma2);
	double gamma3 = det(mgamma3);
	double gamma4 = -1*det(mgamma4);
	double delta1 = -1*det(mdelta1);
	double delta2 = det(mdelta2);
	double delta3 = -1*det(mdelta3);
	double delta4 = det(mdelta4);

	arma::Mat<double> B1 = arma::zeros(3,6);
	B1     << beta1 << 0 << 0 <<  arma::endr
		<< 0 << gamma1 << 0 << arma::endr
		<< 0 << 0 << delta1 <<  arma::endr
		<< gamma1 << beta1 << 0 << arma::endr
		<< 0 << delta1 << gamma1 << arma::endr
		<< delta1 << 0 << beta1 << arma::endr;

	arma::Mat<double> B2 = arma::zeros(3,6);
	B2     << beta2 << 0 << 0 <<  arma::endr
		<< 0 << gamma2 << 0 << arma::endr
		<< 0 << 0 << delta2 <<  arma::endr
		<< gamma2 << beta2 << 0 << arma::endr
		<< 0 << delta2 << gamma2 << arma::endr
		<< delta2 << 0 << beta2 << arma::endr;

	arma::Mat<double> B3 = arma::zeros(3,6);
	B3     << beta3 << 0 << 0 <<  arma::endr
		<< 0 << gamma3 << 0 << arma::endr
		<< 0 << 0 << delta3 <<  arma::endr
		<< gamma3 << beta3 << 0 << arma::endr
		<< 0 << delta3 << gamma3 << arma::endr
		<< delta3 << 0 << beta3 << arma::endr;

	arma::Mat<double> B4 = arma::zeros(3,6);
	B4     << beta4 << 0 << 0 <<  arma::endr
		<< 0 << gamma4 << 0 << arma::endr
		<< 0 << 0 << delta4 <<  arma::endr
		<< gamma4 << beta4 << 0 << arma::endr
		<< 0 << delta4 << gamma4 << arma::endr
		<< delta4 << 0 << beta4 << arma::endr;

	arma::Mat<double> B = arma::zeros(6,6);

	B = arma::join_rows(arma::join_rows(arma::join_rows(B1,B2),B3),B4)/(6*V);

	return B;

}

arma::Mat<double> calculateD(float E,float NU) {

	double a = E*((1-NU)/((1+NU)*(1-2*NU)));
	double b = E*(NU/((1+NU)*(1-2*NU)));
	double c = E*((1-2*NU)/(2*((1+NU)*(1-2*NU))));

	arma::Mat<double> AB;
	arma::Mat<double> C;
	AB << a << b << b << arma::endr
		<< b << a << b << arma::endr
		<< b << b << a << arma::endr;

	C  << c << 0 << 0 << arma::endr
		<< 0 << c << 0 << arma::endr
		<< 0 << 0 << c << arma::endr;

	arma::Mat<double> fillout = arma::zeros(3,3);
	arma::Mat<double> D;
	D = arma::join_cols(arma::join_rows(AB,fillout),arma::join_rows(fillout,C));

	return D;
}